﻿/*
* Copyright (c) 2010-2012 Tesla Engine Group
* 
* Permission is hereby granted, free of charge, to any person obtaining a copy
* of this software and associated documentation files (the "Software"), to deal
* in the Software without restriction, including without limitation the rights
* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
* copies of the Software, and to permit persons to whom the Software is
* furnished to do so, subject to the following conditions:
* 
* The above copyright notice and this permission notice shall be included in
* all copies or substantial portions of the Software.
* 
* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
* THE SOFTWARE.
*/


using System;
using System.Globalization;
using System.Runtime.InteropServices;

namespace Tesla.Math {

    /// <summary>
    /// Describes a mathematical plane with normal vector and plane constant d.
    /// </summary>
    [Serializable]
    [StructLayout(LayoutKind.Sequential)]
    public struct Plane : IEquatable<Plane> {
        /// <summary>
        /// The normal vector of the plane.
        /// </summary>
        public Vector3 Normal;

        /// <summary>
        /// The distance of the plane along its normal from the origin.
        /// </summary>
        public float D;

        /// <summary>
        /// Construct a plane from the specified components.
        /// </summary>
        /// <param name="a">X component of the normal</param>
        /// <param name="b">Y component of the normal</param>
        /// <param name="c">Z component of the normal</param>
        /// <param name="d">Plane constant</param>
        public Plane(float a, float b, float c, float d) {
            Vector3 v;
            v.X = a;
            v.Y = b;
            v.Z = c;
            this.Normal = v;
            this.D = d;
        }

        /// <summary>
        /// Construct a plane from the specified normal
        /// and D values.
        /// </summary>
        /// <param name="n">Normal vector</param>
        /// <param name="d">Plane constant</param>
        public Plane(Vector3 n, float d) {
            this.Normal = n;
            this.D = d;
        }

        /// <summary>
        /// Construct a plane from a Vector4 where (x,y,z)
        /// contain the normal vector, and w contains D.
        /// </summary>
        /// <param name="plane">Vector4 representing the plane</param>
        public Plane(Vector4 plane) {
            Vector3 v;
            v.X = plane.X;
            v.Y = plane.Y;
            v.Z = plane.Z;
            this.Normal = v;
            this.D = plane.W;
        }

        /// <summary>
        /// Construct a plane from three points.
        /// </summary>
        /// <param name="p1">First position</param>
        /// <param name="p2">Second position</param>
        /// <param name="p3">Third position</param>
        public Plane(Vector3 p1, Vector3 p2, Vector3 p3) {
            float x = p1.X;
            float y = p1.Y;
            float z = p1.Z;

            //Compute first vector components
            float dx1 = p2.X - x;
            float dy1 = p2.Y - y;
            float dz1 = p2.Z - z;

            //Compute second vector components
            float dx2 = p3.X - x;
            float dy2 = p3.Y - y;
            float dz2 = p3.Z - z;

            //Take cross product
            float nx = (dy1 * dz2) - (dz1 * dy2);
            float ny = (dz1 * dx2) - (dx1 * dz2);
            float nz = (dx1 * dy2) - (dy1 * dx2);

            //Compute inverse count, to normalize normal
            float invLength = 1.0f / (float) System.Math.Sqrt((nx * nx) + (ny * ny) + (nz * nz));

            //Save normal vector
            this.Normal.X = nx * invLength;
            this.Normal.Y = ny * invLength;
            this.Normal.Z = nz * invLength;

            //Compute plane constant 
            this.D = -((x * Normal.X) + (y * Normal.X) + (z * Normal.Z));
        }

        /// <summary>
        /// Compute the dot product between the specified plane and vector4.
        /// </summary>
        /// <param name="plane">Plane</param>
        /// <param name="value">Vector4</param>
        /// <returns>Dot product</returns>
        public static float Dot(Plane plane, Vector4 value) {
            return (plane.Normal.X * value.X) + (plane.Normal.Y * value.Y) + (plane.Normal.Z * value.Z) + (plane.D * value.W);
        }

        /// <summary>
        /// Compute the dot product between the specified plane and vector4.
        /// </summary>
        /// <param name="plane">Plane</param>
        /// <param name="value">Vector4</param>
        /// <param name="result">Existing float to hold dot product</param>
        public static void Dot(ref Plane plane, ref Vector4 value, out float result) {
            result = (plane.Normal.X * value.X) + (plane.Normal.Y * value.Y) + (plane.Normal.Z * value.Z) + (plane.D * value.W);
        }

        /// <summary>
        /// Compute the dot product between the specified plane's normal and vector3 plus the plane's D component.
        /// </summary>
        /// <param name="plane">Plane</param>
        /// <param name="value">Vector3</param>
        /// <returns>Dot coordinate</returns>
        public static float DotCoordinate(Plane plane, Vector3 value) {
            return (plane.Normal.X * value.X) + (plane.Normal.Y * value.Y) + (plane.Normal.Z * value.Z) + plane.D;
        }

        /// <summary>
        /// Compute the dot product between the specified plane's normal and vector3 plus the plane's D component.
        /// </summary>
        /// <param name="plane">Plane</param>
        /// <param name="value">Vector3</param>
        /// <param name="result">Existing float to hold dot coordinate</param>
        public static void DotCoordinate(ref Plane plane, ref Vector3 value, out float result) {
            result = (plane.Normal.X * value.X) + (plane.Normal.Y * value.Y) + (plane.Normal.Z * value.Z) + plane.D;
        }

        /// <summary>
        /// Compute the dot product between the specified plane's normal and vector3.
        /// </summary>
        /// <param name="plane">Plane</param>
        /// <param name="value">Vector3</param>
        /// <returns>Dot product</returns>
        public static float DotNormal(Plane plane, Vector3 value) {
            return (plane.Normal.X * value.X) + (plane.Normal.Y * value.Y) + (plane.Normal.Z * value.Z);
        }

        /// <summary>
        /// Compute the dot product between the specified plane's normal and vector3.
        /// </summary>
        /// <param name="plane">Plane</param>
        /// <param name="value">Vector3</param>
        /// <param name="result">Existing float to hold dot product</param>
        public static void DotNormal(ref Plane plane, ref Vector3 value, out float result) {
            result = (plane.Normal.X * value.X) + (plane.Normal.Y * value.Y) + (plane.Normal.Z * value.Z);
        }

        /// <summary>
        /// Normalize this plane so its normal of unit count.
        /// </summary>
        public void Normalize() {
            float lengthSquared = (this.Normal.X * this.Normal.X) + (this.Normal.Y * this.Normal.Y) + (this.Normal.Z * this.Normal.Z);
            if(System.Math.Abs((double) (lengthSquared - 1.0f)) >= 1.192093E-07f) {
                float invLength = 1.0f / (float) System.Math.Sqrt((double) lengthSquared);
                this.Normal.X *= invLength;
                this.Normal.Y *= invLength;
                this.Normal.Z *= invLength;
                this.D *= invLength;
            }
        }

        /// <summary>
        /// Normalize a plane's normal vector to unit count.
        /// </summary>
        /// <param name="plane">Plane to normalize</param>
        /// <returns>Normalized plane</returns>
        public static Plane Normalize(Plane plane) {
            Plane p;
            float lengthSquared = (plane.Normal.X * plane.Normal.X) + (plane.Normal.Y * plane.Normal.Y) + (plane.Normal.Z * plane.Normal.Z);
            if(System.Math.Abs((double) (lengthSquared - 1.0f)) < 1.192093E-07f) {
                p.Normal.X = plane.Normal.X;
                p.Normal.Y = plane.Normal.Y;
                p.Normal.Z = plane.Normal.Z;
                p.D = plane.D;
                return p;
            } else {
                float invLength = 1.0f / (float) System.Math.Sqrt((double) lengthSquared);
                p.Normal.X = plane.Normal.X * invLength;
                p.Normal.Y = plane.Normal.Y * invLength;
                p.Normal.Z = plane.Normal.Z * invLength;
                p.D = plane.D * invLength;
                return p;
            }
        }

        /// <summary>
        /// Normalize a plane's normal vector to unit count.
        /// </summary>
        /// <param name="plane">Plane to normalize</param>
        /// <param name="result">Existing plane to hold result</param>
        public static void Normalize(ref Plane plane, out Plane result) {
            float lengthSquared = (plane.Normal.X * plane.Normal.X) + (plane.Normal.Y * plane.Normal.Y) + (plane.Normal.Z * plane.Normal.Z);
            if(System.Math.Abs((double) (lengthSquared - 1.0f)) < 1.192093E-07f) {
                result.Normal.X = plane.Normal.X;
                result.Normal.Y = plane.Normal.Y;
                result.Normal.Z = plane.Normal.Z;
                result.D = plane.D;
            } else {
                float invLength = 1.0f / (float) System.Math.Sqrt((double) lengthSquared);
                result.Normal.X = plane.Normal.X * invLength;
                result.Normal.Y = plane.Normal.Y * invLength;
                result.Normal.Z = plane.Normal.Z * invLength;
                result.D = plane.D * invLength;
            }
        }

        /// <summary>
        /// Transforms a normalized plane by a matrix. The Plane MUST be
        /// normalized before using this method.
        /// </summary>
        /// <param name="plane">Normalized plane</param>
        /// <param name="matrix">Transformation matrix</param>
        /// <returns>Transformed plane</returns>
        public static Plane Transform(Plane plane, Matrix matrix) {
            Plane p;
            Matrix m;
            Matrix.Invert(ref matrix, out m);
            float x = plane.Normal.X;
            float y = plane.Normal.Y;
            float z = plane.Normal.Z;
            float d = plane.D;

            p.Normal.X = (x * m.M11) + (y * m.M12) + (z * m.M13) + (d * m.M14);
            p.Normal.Y = (x * m.M21) + (y * m.M22) + (z * m.M23) + (d * m.M24);
            p.Normal.Z = (x * m.M31) + (y * m.M32) + (z * m.M33) + (d * m.M34);
            p.D = (x * m.M41) + (y * m.M42) + (z * m.M43) + (d * m.M44);
            return p;
        }

        /// <summary>
        /// Transforms a normalized plane by a matrix. The Plane MUST be
        /// normalized before using this method.
        /// </summary>
        /// <param name="plane">Normalized plane</param>
        /// <param name="matrix">Transformation matrix</param>
        /// <param name="result">Existing plane to hold result</param>
        public static void Transform(ref Plane plane, ref Matrix matrix, out Plane result) {
            Matrix m;
            Matrix.Invert(ref matrix, out m);
            float x = plane.Normal.X;
            float y = plane.Normal.Y;
            float z = plane.Normal.Z;
            float d = plane.D;

            result.Normal.X = (x * m.M11) + (y * m.M12) + (z * m.M13) + (d * m.M14);
            result.Normal.Y = (x * m.M21) + (y * m.M22) + (z * m.M23) + (d * m.M24);
            result.Normal.Z = (x * m.M31) + (y * m.M32) + (z * m.M33) + (d * m.M34);
            result.D = (x * m.M41) + (y * m.M42) + (z * m.M43) + (d * m.M44);
        }

        /// <summary>
        /// Transforms a normalized plane by a rotation quaternion. The Plane
        /// MUST be normalized before using this method.
        /// </summary>
        /// <param name="plane">Normalized plane</param>
        /// <param name="q">Rotation quaternion</param>
        /// <returns>Rotated plane</returns>
        public static Plane Transform(Plane plane, Quaternion q) {
            Plane p;

            float x2 = q.X + q.X;
            float y2 = q.Y + q.Y;
            float z2 = q.Z + q.Z;
            float wx2 = q.W * x2;
            float wy2 = q.W * y2;
            float wz2 = q.W * z2;

            float xx2 = q.X * x2;
            float xy2 = q.X * y2;
            float xz2 = q.X * z2;

            float yy2 = q.Y * y2;
            float yz2 = q.Y * z2;
            float zz2 = q.Z * z2;

            float h1 = (1f - yy2) - zz2;
            float h2 = xy2 - wz2;
            float h3 = xz2 + wy2;
            float h4 = xy2 + wz2;
            float h5 = (1f - xx2) - zz2;
            float h6 = yz2 - wx2;
            float h7 = xz2 - wy2;
            float h8 = yz2 + wx2;
            float h9 = (1f - xx2) - yy2;

            float x = plane.Normal.X;
            float y = plane.Normal.Y;
            float z = plane.Normal.Z;

            p.Normal.X = ((x * h1) + (y * h2)) + (z * h3);
            p.Normal.Y = ((x * h4) + (y * h5)) + (z * h6);
            p.Normal.Z = ((x * h7) + (y * h8)) + (z * h9);
            p.D = plane.D;

            return p;
        }

        /// <summary>
        /// Transforms a normalized plane by a rotation quaternion. The Plane
        /// MUST be normalized before using this method.
        /// </summary>
        /// <param name="plane">Normalized plane</param>
        /// <param name="q">Rotation quaternion</param>
        /// <param name="result">Existing plane to hold result</param>
        public static void Transform(ref Plane plane, ref Quaternion q, out Plane result) {
            float x2 = q.X + q.X;
            float y2 = q.Y + q.Y;
            float z2 = q.Z + q.Z;
            float wx2 = q.W * x2;
            float wy2 = q.W * y2;
            float wz2 = q.W * z2;

            float xx2 = q.X * x2;
            float xy2 = q.X * y2;
            float xz2 = q.X * z2;

            float yy2 = q.Y * y2;
            float yz2 = q.Y * z2;
            float zz2 = q.Z * z2;

            float h1 = (1f - yy2) - zz2;
            float h2 = xy2 - wz2;
            float h3 = xz2 + wy2;
            float h4 = xy2 + wz2;
            float h5 = (1f - xx2) - zz2;
            float h6 = yz2 - wx2;
            float h7 = xz2 - wy2;
            float h8 = yz2 + wx2;
            float h9 = (1f - xx2) - yy2;

            float x = plane.Normal.X;
            float y = plane.Normal.Y;
            float z = plane.Normal.Z;

            result.Normal.X = ((x * h1) + (y * h2)) + (z * h3);
            result.Normal.Y = ((x * h4) + (y * h5)) + (z * h6);
            result.Normal.Z = ((x * h7) + (y * h8)) + (z * h9);
            result.D = plane.D;
        }

        /// <summary>
        /// Tests whether this Plane is equal to the supplied Plane.
        /// </summary>
        /// <param name="value">Plane to compare</param>
        /// <returns>If equal</returns>
        public bool Equals(Plane value) {
            return ((((this.Normal.X == value.Normal.X) && (this.Normal.Y == value.Normal.Y)) && (this.Normal.Z == value.Normal.Z)) && (this.D == value.D));
        }

        /// <summary>
        /// Tests whether this object is equal to the supplied object.
        /// </summary>
        /// <param name="obj">Object to compare</param>
        /// <returns>If equal</returns>
        public override bool Equals(object obj) {
            if(obj is Plane) {
                return Equals((Plane) obj);
            }
            return false;
        }

        /// <summary>
        /// Get the hash code for this object.
        /// </summary>
        /// <returns>Hash code</returns>
        public override int GetHashCode() {
            return this.Normal.GetHashCode() + this.D.GetHashCode();
        }

        /// <summary>
        /// Tests whether two instances of Plane are equal.
        /// </summary>
        /// <param name="a">First plane</param>
        /// <param name="b">Second plane</param>
        /// <returns>If equal</returns>
        public static bool operator==(Plane a, Plane b) {
            return ((((a.Normal.X == b.Normal.X) && (a.Normal.Y == b.Normal.Y)) && (a.Normal.Z == b.Normal.Z)) && (a.D == b.D));
        }

        /// <summary>
        /// Tests whether two instances of Plane are not equal.
        /// </summary>
        /// <param name="a">First plane</param>
        /// <param name="b">Second plane</param>
        /// <returns>If not equal</returns>
        public static bool operator!=(Plane a, Plane b) {
            return !(a == b);
        }

        /// <summary>
        /// Get the string repesentation of this object.
        /// </summary>
        /// <returns></returns>
        public override string ToString() {
            CultureInfo currentCulture = CultureInfo.CurrentCulture;
            return String.Format(currentCulture, "{{Normal:{0} D:{1}}}", new Object[] { this.Normal.ToString(), this.D.ToString(currentCulture) });
        }
    }
}
